#ifndef QUATERNION_H
#define QUATERNION_H

#include <cassert>
#include "Prerequisites.h"
#include "Math3D.h"


class Matrix3;

/** Implementation of a Quaternion, i.e. a rotation around an axis.
*/
class Quaternion
{
public:
	inline Quaternion (float fX = 0.0f, float fY = 0.0f, float fZ = 0.0f, float fW = 1.0f)
	{
		x = fX;
		y = fY;
		z = fZ;
		w = fW;
	}
	inline Quaternion (const Quaternion& rkQ)
	{
		x = rkQ.x;
		y = rkQ.y;
		z = rkQ.z;
		w = rkQ.w;
	}
	/// Construct a quaternion from a rotation matrix
	inline Quaternion(const Matrix3& rot)
	{
		this->FromRotationMatrix(rot);
	}
	/// Construct a quaternion from an angle/axis
	inline Quaternion(const Radian& rfAngle, const Vector3D& rkAxis)
	{
		this->FromAngleAxis(rfAngle, rkAxis);
	}
	/// Construct a quaternion from 3 orthonormal local axes
	inline Quaternion(const Vector3D& xaxis, const Vector3D& yaxis, const Vector3D& zaxis)
	{
		this->FromAxes(xaxis, yaxis, zaxis);
	}
	/// Construct a quaternion from 3 orthonormal local axes
	inline Quaternion(const Vector3D* akAxis)
	{
		this->FromAxes(akAxis);
	}
	/// Construct a quaternion from 4 manual w/x/y/z values
	inline Quaternion(float* valptr)
	{
		memcpy(&x, valptr, sizeof(float)*4);
	}

	/// Array accessor operator
	inline float operator [] ( const size_t i ) const
	{
		assert( i < 4 );

		return *(&x+i);
	}

	/// Array accessor operator
	inline float& operator [] ( const size_t i )
	{
		assert( i < 4 );

		return *(&x+i);
	}

	/// Pointer accessor for direct copying
	inline float* ptr()
	{
		return &x;
	}

	/// Pointer accessor for direct copying
	inline const float* ptr() const
	{
		return &x;
	}

	void FromRotationMatrix (const Matrix3& kRot);
	void ToRotationMatrix (Matrix3& kRot) const;
	void FromAngleAxis (const Radian& rfAngle, const Vector3D& rkAxis);
	void ToAngleAxis (Radian& rfAngle, Vector3D& rkAxis) const;
	inline void ToAngleAxis (Degree& dAngle, Vector3D& rkAxis) const {
		Radian rAngle;
		ToAngleAxis ( rAngle, rkAxis );
		dAngle = rAngle;
	}
	void FromAxes (const Vector3D* akAxis);
	void FromAxes (const Vector3D& xAxis, const Vector3D& yAxis, const Vector3D& zAxis);
	void ToAxes (Vector3D* akAxis) const;
	void ToAxes (Vector3D& xAxis, Vector3D& yAxis, Vector3D& zAxis) const;
	/// Get the local x-axis
	Vector3D xAxis(void) const;
	/// Get the local y-axis
	Vector3D yAxis(void) const;
	/// Get the local z-axis
	Vector3D zAxis(void) const;

	inline Quaternion& operator= (const Quaternion& rkQ)
	{
		x = rkQ.x;
		y = rkQ.y;
		z = rkQ.z;
		w = rkQ.w;
		return *this;
	}
	Quaternion operator+ (const Quaternion& rkQ) const;
	Quaternion operator- (const Quaternion& rkQ) const;
	Quaternion operator* (const Quaternion& rkQ) const;
	Quaternion operator* (float fScalar) const;
	friend Quaternion operator* (float fScalar,
		const Quaternion& rkQ);
	Quaternion operator- () const;
	inline bool operator== (const Quaternion& rhs) const
	{
		return (rhs.x == x) && (rhs.y == y) &&
			(rhs.z == z) && (rhs.w == w);
	}
	inline bool operator!= (const Quaternion& rhs) const
	{
		return !operator==(rhs);
	}
	// functions of a quaternion
	float Dot (const Quaternion& rkQ) const;  // dot product
	float Norm () const;  // squared-length
	/// Normalises this quaternion, and returns the previous length
	Quaternion& normalize(void);
	Quaternion Inverse () const;  // apply to non-zero quaternion
	Quaternion UnitInverse () const;  // apply to unit-length quaternion
	Quaternion Exp () const;
	Quaternion Log () const;

	// rotation of a vector by a quaternion
	Vector3D operator* (const Vector3D& rkVector) const;

	/** Calculate the local roll element of this quaternion.
	@param reprojectAxis By default the method returns the 'intuitive' result
		that is, if you projected the local Y of the quaterion onto the X and
		Y axes, the angle between them is returned. If set to false though, the
		result is the actual yaw that will be used to implement the quaternion,
		which is the shortest possible path to get to the same orientation and
		may involve less axial rotation.
	*/
	Radian getRoll(bool reprojectAxis = true) const;
	/** Calculate the local pitch element of this quaternion
	@param reprojectAxis By default the method returns the 'intuitive' result
		that is, if you projected the local Z of the quaterion onto the X and
		Y axes, the angle between them is returned. If set to true though, the
		result is the actual yaw that will be used to implement the quaternion,
		which is the shortest possible path to get to the same orientation and
		may involve less axial rotation.
	*/
	Radian getPitch(bool reprojectAxis = true) const;
	/** Calculate the local yaw element of this quaternion
	@param reprojectAxis By default the method returns the 'intuitive' result
		that is, if you projected the local Z of the quaterion onto the X and
		Z axes, the angle between them is returned. If set to true though, the
		result is the actual yaw that will be used to implement the quaternion,
		which is the shortest possible path to get to the same orientation and
		may involve less axial rotation.
	*/
	Radian getYaw(bool reprojectAxis = true) const;
	/// Equality with tolerance (tolerance is max angle difference)
	bool equals(const Quaternion& rhs, const Radian& tolerance) const;

	// spherical linear interpolation
	static Quaternion Slerp (float fT, const Quaternion& rkP,
		const Quaternion& rkQ, bool shortestPath = false);

	static Quaternion SlerpExtraSpins (float fT,
		const Quaternion& rkP, const Quaternion& rkQ,
		int iExtraSpins);

	// setup for spherical quadratic interpolation
	static void Intermediate (const Quaternion& rkQ0,
		const Quaternion& rkQ1, const Quaternion& rkQ2,
		Quaternion& rka, Quaternion& rkB);

	// spherical quadratic interpolation
	static Quaternion Squad (float fT, const Quaternion& rkP,
		const Quaternion& rkA, const Quaternion& rkB,
		const Quaternion& rkQ, bool shortestPath = false);

	// normalised linear interpolation - faster but less accurate (non-constant rotation velocity)
	static Quaternion nlerp(float fT, const Quaternion& rkP,
		const Quaternion& rkQ, bool shortestPath = false);

	// cutoff for sine near zero
	static const float ms_fEpsilon;

	// special values
	static const Quaternion ZERO;
	static const Quaternion IDENTITY;

	float x, y, z, w;

	/** Function for writing to a stream. Outputs "Quaternion(w, x, y, z)" with w,x,y,z
		being the member values of the quaternion.
	*/
	inline friend std::ostream& operator <<
		( std::ostream& o, const Quaternion& q )
	{
		o << "Quaternion(" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << ")";
		return o;
	}

	static const Quaternion slerp(const float r, const Quaternion &q1, const Quaternion &q2)
	{
		// SLERP
		float dot = q1.Dot(q2);

		if (fabs(dot) > 0.9995f) {
			// fall back to LERP
			return Quaternion::lerp(r, q1, q2);
		}

		float a = acosf(dot) * r;
		Quaternion q = (q2 - q1 * dot);
		q.normalize();

		return q1 * cosf(a) + q * sinf(a);
	}

	static const Quaternion lerp(const float r, const Quaternion &q1, const Quaternion &q2)
	{
		return q1*(1.0f-r) + q2*r;
	}
};

// "linear" interpolation for quaternions should be slerp by default
template<>
inline Quaternion interpolate<Quaternion>(const float r, const Quaternion &q1, const Quaternion &q2)
{
	return Quaternion::Slerp(r, q1, q2);
}

#endif
